/*
 * ZDTDriver.c
 *
 *  Created on: Nov 18, 2023
 *      Author: 26090
 */
#include "ZDTDriver.h"
#include "stdbool.h"
#include "stdlib.h"
#include "string.h"

#define delay_ms(ms) HAL_Delay(ms)

#define bsp_uart_send(instance,buf,size) HAL_UART_Transmit_DMA(instance, buf, size)

#define BUFFERSIZE 50u

#define ZDT_ISNEGATIVE(num) (num==0)?(1):(-1)

#define ZDT_NUM 2

static UART_HandleTypeDef *zdt_uart;

static ZDTInstance *zdtlist[ZDT_NUM];

ZDTInstance zdt0,zdt1,zdt2;

volatile uint16_t receive_size=20;

bool zdt_lock=false;

uint8_t zdt_buffer[BUFFERSIZE];

static void ZDT_SendCmd(ZDTInstance *zdt, uint8_t *cmd, uint8_t size);

static void ZDT_ReceiveProcess(uint8_t *buf);

static ZDTReturn_t ZDT_UnPack(uint8_t data[]);


void ZDT_Init(UART_HandleTypeDef *uart)
{
	zdt_uart=uart;

	//bsp_uart_init(zdt_uart,ZDT_ReceiveProcess);


	
	//id0ȫ����
	zdtlist[0]=&zdt0;
	memset(zdtlist[0],0,sizeof(ZDTInstance));
	zdtlist[0]->id=0;
	zdtlist[0]->received_flag=1;

	//����Ϊ�ֱ���
	zdtlist[1]=&zdt1;
	memset(zdtlist[1],0,sizeof(ZDTInstance));
	zdtlist[1]->id=1;
	zdtlist[1]->received_flag=1;

	// zdtlist[2]=&zdt2;
	// memset(zdtlist[2],0,sizeof(ZDTInstance));
	// zdtlist[2]->id=2;
	// zdtlist[2]->received_flag=1;

	delay_ms(10);
	
	ZDT_Reset_CurPos_To_Zero(&zdt0);
	delay_ms(10);
	
	ZDT_Set_Sys_PID(zdtlist[1],false,0,0,0);
	delay_ms(20);
	

}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
	if (huart == zdt_uart)
	{
		if (Size >= receive_size)
		HAL_UART_AbortReceive(zdt_uart);
		ZDT_ReceivePro();
	}
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
	if (huart == zdt_uart)
	{
		__HAL_UNLOCK(huart);
		__HAL_UART_CLEAR_FLAG(zdt_uart,UART_FLAG_ORE);
		HAL_UART_AbortReceive(zdt_uart);
	}
}

void ZDT_SendCmd(ZDTInstance *zdt, uint8_t cmd[], uint8_t size)
{
	//while(zdt_lock);
	static uint8_t txbuff[BUFFERSIZE];
	memcpy(txbuff, cmd, size);
	bsp_uart_send(zdt_uart, txbuff, size);

}

//��ʱ���ͻ�ȡλ����Ϣ
void ZDTGetPos(uint16_t id)
{
	if(zdtlist[id]->received_flag)
	{
		static ZDTSend zdtsend;
		zdtsend.instance = zdtlist[id];
		zdtsend.Param = S_CPOS;

		ZDT_Read_Sys_Params(zdtsend.instance, zdtsend.Param);
		zdtlist[id]->received_flag=0;
	}
}

void ZDTGetVel(uint16_t id)
{
	if(zdtlist[id]->received_flag)
	{
		static ZDTSend zdtsend;
		zdtsend.instance = zdtlist[id];
		zdtsend.Param = S_VEL;

		ZDT_Read_Sys_Params(zdtsend.instance, zdtsend.Param);
		zdtlist[id]->received_flag=0;
	}
}

uint8_t databuf[BUFFERSIZE];
//这个是不需要传入buffer的
void ZDT_ReceivePro()
{
	memcpy(databuf,zdt_buffer,receive_size);
	zdt_lock=false;
	if (databuf[receive_size - 1] == 0x6B)
	{
		ZDTReturn_t state = ZDT_UnPack(databuf);
		return;
	}
	else
	{
		for (int i =0;i<ZDT_NUM;i++)
		{
			zdtlist[i]->received_flag=1;
		}
	}
	memset(databuf, 0, receive_size);
	return;
}


//����ʵ�����ջص�����
void ZDT_ReceiveProcess(uint8_t *buf)
{
	memcpy(databuf,buf,receive_size);
	zdt_lock=false;

	if (databuf[receive_size - 1] == 0x6B)
	{
		ZDTReturn_t state = ZDT_UnPack(databuf);
		return;
	}
	else
	{
		for (int i =0;i<ZDT_NUM;i++)
		{
			zdtlist[i]->received_flag=1;
		}
	}
	memset(databuf, 0, receive_size);
	return;
}

ZDTReturn_t ZDT_UnPack(uint8_t data[])
{
	uint8_t id;
	ZDTCommand_t command;
	id = databuf[0];
	command = databuf[1];

	if (zdtlist[id] == NULL)
	{
		return ZDT_ID_ERROR;
	}
	zdtlist[id]->received_flag=1;

	switch (command)
	{

	case ZDT_PULSE:
		zdtlist[id]->state.Pulse =
		ZDT_ISNEGATIVE(databuf[2]) * ((databuf[3] << 24) | (databuf[4] << 16) | (databuf[5] << 8) | (databuf[6]));
		break;
	case ZDT_VEL:
		zdtlist[id]->state.CurrentSpeed = (int16_t) ((databuf[3] << 8) | (databuf[4]))*((databuf[2]==1)?-1:1);
		break;

	case ZDT_CPOS:
		zdtlist[id]->state.CurrentPosition = ((databuf[3] << 24)|(databuf[4] << 16)|(databuf[5] << 8) | (databuf[6]))*((databuf[2]==1)?-1:1);

		break;
	case ZDT_ERROR:
		if (databuf[2] == 0xee)
			return ZDT_COMMAND_ERROR;
	default:
		return ZDT_COMMAND_ERROR;
	}
	return ZDT_OK;
}

/**
 * @brief    ����ǰλ������
 * @param    addr  �������ַ
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_Reset_CurPos_To_Zero(ZDTInstance *zdt)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                 // ��ַ
	cmd[1] = 0x0A;                       // ������
	cmd[2] = 0x6D;                       // ������
	cmd[3] = 0x6B;                       // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 4);
}
//
///**
// * @brief    �����ת����
// * @param    addr  �������ַ
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Reset_Clog_Pro(uint8_t addr)
//{
//	uint8_t cmd[16] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x0E;                       // ������
//	cmd[2] = 0x52;                       // ������
//	cmd[3] = 0x6B;                       // У���ֽ�
//
//	// ��������
//	usart_SendCmd(cmd, 4);
//}

/**
 * @brief    ��ȡϵͳ����
 * @param    addr  �������ַ
 * @param    s     ��ϵͳ��������
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_Read_Sys_Params(ZDTInstance *zdt, SysParams_t s)
{
	volatile uint8_t i = 0;
	uint8_t cmd[16] = { 0 };
	

	// װ������
	cmd[i] = zdt->id;
	++i;                   // ��ַ

	switch (s)
	// ������
	{
	case S_VER:
		cmd[i] = 0x1F;
		++i;
		break;
	case S_RL:
		cmd[i] = 0x20;
		++i;
		break;
	case S_PID:
		cmd[i] = 0x21;
		++i;
		break;
	case S_VBUS:
		cmd[i] = 0x24;
		++i;
		break;
	case S_CPHA:
		cmd[i] = 0x27;
		++i;
		break;
	case S_ENCL:
		cmd[i] = 0x31;
		++i;
		receive_size=5;
		break;
	case S_TPOS:
		cmd[i] = 0x33;
		++i;
		break;
	case S_VEL:
		cmd[i] = 0x35;
		++i;
		receive_size=6;
		break;
	case S_CPOS:
		cmd[i] = 0x36;
		++i;
		receive_size=8;
		break;
	case S_PERR:
		cmd[i] = 0x37;
		++i;
		break;
	case S_FLAG:
		cmd[i] = 0x3A;
		++i;
		break;
	case S_ORG:
		cmd[i] = 0x3B;
		++i;
		break;
	case S_Conf:
		cmd[i] = 0x42;
		++i;
		cmd[i] = 0x6C;
		++i;
		break;
	case S_State:
		cmd[i] = 0x43;
		++i;
		cmd[i] = 0x7A;
		++i;
		break;
	case S_PULSE:
		cmd[i] = 0x32;
		++i;
		break;
	default:
		break;
	}

	cmd[i] = 0x6B;
	++i;                   // У���ֽ�
	
	//while(zdt_lock);
	// ��������
	ZDT_SendCmd(zdt, cmd, i);

	//bsp_uart_start_receive(zdt_uart,receive_size);
	__HAL_DMA_DISABLE_IT(zdt_uart->hdmarx, DMA_IT_HT);
	HAL_UARTEx_ReceiveToIdle_DMA(zdt_uart, zdt_buffer, receive_size);
}

///**
// * @brief    �޸Ŀ���/�ջ�����ģʽ
// * @param    addr     �������ַ
// * @param    svF      ���Ƿ�洢��־��falseΪ���洢��trueΪ�洢
// * @param    ctrl_mode������ģʽ����Ӧ��Ļ�ϵ�P_Pul�˵�����0�ǹر������������ţ�1�ǿ���ģʽ��2�Ǳջ�ģʽ��3����En�˿ڸ���Ϊ��Ȧ��λ�����������ţ�Dir�˿ڸ���Ϊ��λ����ߵ�ƽ����
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode)
//{
//	uint8_t cmd[16] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x46;                       // ������
//	cmd[2] = 0x69;                       // ������
//	cmd[3] = svF;                        // �Ƿ�洢��־��falseΪ���洢��trueΪ�洢
//	cmd[4] = ctrl_mode;           // ����ģʽ����Ӧ��Ļ�ϵ�P_Pul�˵�����0�ǹر������������ţ�1�ǿ���ģʽ��2�Ǳջ�ģʽ��3����En�˿ڸ���Ϊ��Ȧ��λ�����������ţ�Dir�˿ڸ���Ϊ��λ����ߵ�ƽ����
//	cmd[5] = 0x6B;                       // У���ֽ�
//
//	// ��������
//	usart_SendCmd(cmd, 6);
//}

/**
 * @brief    ʹ���źſ���
 * @param    addr  �������ַ
 * @param    state ��ʹ��״̬     ��trueΪʹ�ܵ����falseΪ�رյ��
 * @param    snF   �����ͬ����־ ��falseΪ�����ã�trueΪ����
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_En_Control(ZDTInstance *zdt, bool state, bool snF)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                      // ��ַ
	cmd[1] = 0xF3;                       // ������
	cmd[2] = 0xAB;                       // ������
	cmd[3] = (uint8_t) state;             // ʹ��״̬
	cmd[4] = snF;                        // ���ͬ���˶���־
	cmd[5] = 0x6B;                       // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 6);
}

/**
 * @brief    �ٶ�ģʽ
 * @param    addr�������ַ
 * @param    dir ������       ��0ΪCW������ֵΪCCW
 * @param    vel ���ٶ�       ����Χ0 - 5000RPM
 * @param    acc �����ٶ�     ����Χ0 - 255��ע�⣺0��ֱ������
 * @param    snF �����ͬ����־��falseΪ�����ã�trueΪ����
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_Vel_Control(ZDTInstance *zdt, uint8_t dir, uint16_t vel, uint8_t acc, bool snF)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                      // ��ַ
	cmd[1] = 0xF6;                       // ������
	cmd[2] = dir;                        // ����
	cmd[3] = (uint8_t) (vel >> 8);        // �ٶ�(RPM)��8λ�ֽ�
	cmd[4] = (uint8_t) (vel >> 0);        // �ٶ�(RPM)��8λ�ֽ�
	cmd[5] = acc;                        // ���ٶȣ�ע�⣺0��ֱ������
	cmd[6] = snF;                        // ���ͬ���˶���־
	cmd[7] = 0x6B;                       // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 8);
}

/**
 * @brief    λ��ģʽ
 * @param    addr�������ַ
 * @param    dir ������        ��0ΪCW������ֵΪCCW
 * @param    vel ���ٶ�(RPM)   ����Χ0 - 5000RPM
 * @param    acc �����ٶ�      ����Χ0 - 255��ע�⣺0��ֱ������
 * @param    clk ��������      ����Χ0- (2^32 - 1)��
 * @param    raF ����λ/���Ա�־��falseΪ����˶���trueΪ����ֵ�˶�
 * @param    snF �����ͬ����־ ��falseΪ�����ã�trueΪ����
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */

void ZDT_Pos_Control(ZDTInstance *zdt, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                     // ��ַ
	cmd[1] = 0xFD;                      // ������
	cmd[2] = dir;                       // ����
	cmd[3] = (uint8_t) (vel >> 8);       // �ٶ�(RPM)��8λ�ֽ�
	cmd[4] = (uint8_t) (vel >> 0);       // �ٶ�(RPM)��8λ�ֽ�
	cmd[5] = acc;                       // ���ٶȣ�ע�⣺0��ֱ������
	cmd[6] = (uint8_t) (clk >> 24);      // ������(bit24 - bit31)
	cmd[7] = (uint8_t) (clk >> 16);      // ������(bit16 - bit23)
	cmd[8] = (uint8_t) (clk >> 8);       // ������(bit8  - bit15)
	cmd[9] = (uint8_t) (clk >> 0);       // ������(bit0  - bit7 )
	cmd[10] = raF;                       // ��λ/���Ա�־��falseΪ����˶���trueΪ����ֵ�˶�
	cmd[11] = snF;                       // ���ͬ���˶���־��falseΪ�����ã�trueΪ����
	cmd[12] = 0x6B;                      // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 13);
}

/**
 * @brief    ����ֹͣ�����п���ģʽ��ͨ�ã�
 * @param    addr  �������ַ
 * @param    snF   �����ͬ����־��falseΪ�����ã�trueΪ����
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_Stop_Now(ZDTInstance *zdt, bool snF)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                      // ��ַ
	cmd[1] = 0xFE;                       // ������
	cmd[2] = 0x98;                       // ������
	cmd[3] = snF;                        // ���ͬ���˶���־
	cmd[4] = 0x6B;                       // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 5);
}

/**
 * @brief    ���ͬ���˶�
 * @param    addr  �������ַ
 * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
 */
void ZDT_Synchronous_motion(ZDTInstance *zdt)
{
	uint8_t cmd[16] = { 0 };

	// װ������
	cmd[0] = zdt->id;                      // ��ַ
	cmd[1] = 0xFF;                       // ������
	cmd[2] = 0x66;                       // ������
	cmd[3] = 0x6B;                       // У���ֽ�

	// ��������
	ZDT_SendCmd(zdt, cmd, 4);
}

void ZDT_Set_Sys_PID(ZDTInstance *zdt, bool save,uint32_t kp,uint32_t ki,uint32_t kd)
{
	uint8_t cmd[17] = { 0 };

	// װ������
	cmd[0] = zdt->id;                      // ��ַ
	cmd[1] = 0x4A;                       // 
	cmd[2] = 0xC3;                       // 
	cmd[3] = save;						//�Ƿ�洢
	
	cmd[4] = (uint8_t) (kp >> 24);
	cmd[5] = (uint8_t) (kp >> 16);
	cmd[6] = (uint8_t) (kp >> 8);
	cmd[7] = (uint8_t) (kp >> 0);
	
	cmd[8] = (uint8_t) (ki >> 24);
	cmd[9] = (uint8_t) (ki >> 16);
	cmd[10] = (uint8_t) (ki >> 8);
	cmd[11] = (uint8_t) (ki >> 0);
	
	cmd[12] = (uint8_t) (kd >> 24);
	cmd[13] = (uint8_t) (kd >> 16);
	cmd[14] = (uint8_t) (kd >> 8);
	cmd[15] = (uint8_t) (kd >> 0);
	
	cmd[16] = 0x6B;
	// ��������
	ZDT_SendCmd(zdt, cmd, 17);
}
//
///**
// * @brief    ���õ�Ȧ��������λ��
// * @param    addr  �������ַ
// * @param    svF   ���Ƿ�洢��־��falseΪ���洢��trueΪ�洢
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Origin_Set_O(ZDTInstance *zdt, bool svF)
//{
//	uint8_t cmd[16] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x93;                       // ������
//	cmd[2] = 0x88;                       // ������
//	cmd[3] = svF;                        // �Ƿ�洢��־��falseΪ���洢��trueΪ�洢
//	cmd[4] = 0x6B;                       // У���ֽ�
//
//	// ��������
//	ZDT_SendCmd(zdt, cmd, 5);
//}

///**
// * @brief    �޸Ļ������
// * @param    addr  �������ַ
// * @param    svF   ���Ƿ�洢��־��falseΪ���洢��trueΪ�洢
// * @param    o_mode ������ģʽ��0Ϊ��Ȧ�ͽ����㣬1Ϊ��Ȧ������㣬2Ϊ��Ȧ����λ��ײ���㣬3Ϊ��Ȧ����λ���ػ���
// * @param    o_dir  �����㷽��0ΪCW������ֵΪCCW
// * @param    o_vel  �������ٶȣ���λ��RPM��ת/���ӣ�
// * @param    o_tm   �����㳬ʱʱ�䣬��λ������
// * @param    sl_vel ������λ��ײ������ת�٣���λ��RPM��ת/���ӣ�
// * @param    sl_ma  ������λ��ײ�������������λ��Ma��������
// * @param    sl_ms  ������λ��ײ������ʱ�䣬��λ��Ms�����룩
// * @param    potF   ���ϵ��Զ��������㣬falseΪ��ʹ�ܣ�trueΪʹ��
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm,
//		uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF)
//{
//	uint8_t cmd[32] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x4C;                       // ������
//	cmd[2] = 0xAE;                       // ������
//	cmd[3] = svF;                        // �Ƿ�洢��־��falseΪ���洢��trueΪ�洢
//	cmd[4] = o_mode;                     // ����ģʽ��0Ϊ��Ȧ�ͽ����㣬1Ϊ��Ȧ������㣬2Ϊ��Ȧ����λ��ײ���㣬3Ϊ��Ȧ����λ���ػ���
//	cmd[5] = o_dir;                      // ���㷽��
//	cmd[6] = (uint8_t) (o_vel >> 8);     // �����ٶ�(RPM)��8λ�ֽ�
//	cmd[7] = (uint8_t) (o_vel >> 0);     // �����ٶ�(RPM)��8λ�ֽ�
//	cmd[8] = (uint8_t) (o_tm >> 24);     // ���㳬ʱʱ��(bit24 - bit31)
//	cmd[9] = (uint8_t) (o_tm >> 16);     // ���㳬ʱʱ��(bit16 - bit23)
//	cmd[10] = (uint8_t) (o_tm >> 8);      // ���㳬ʱʱ��(bit8  - bit15)
//	cmd[11] = (uint8_t) (o_tm >> 0);      // ���㳬ʱʱ��(bit0  - bit7 )
//	cmd[12] = (uint8_t) (sl_vel >> 8);    // ����λ��ײ������ת��(RPM)��8λ�ֽ�
//	cmd[13] = (uint8_t) (sl_vel >> 0);    // ����λ��ײ������ת��(RPM)��8λ�ֽ�
//	cmd[14] = (uint8_t) (sl_ma >> 8);     // ����λ��ײ���������(Ma)��8λ�ֽ�
//	cmd[15] = (uint8_t) (sl_ma >> 0);     // ����λ��ײ���������(Ma)��8λ�ֽ�
//	cmd[16] = (uint8_t) (sl_ms >> 8);     // ����λ��ײ������ʱ��(Ms)��8λ�ֽ�
//	cmd[17] = (uint8_t) (sl_ms >> 0);     // ����λ��ײ������ʱ��(Ms)��8λ�ֽ�
//	cmd[18] = potF;                      // �ϵ��Զ��������㣬falseΪ��ʹ�ܣ�trueΪʹ��
//	cmd[19] = 0x6B;                      // У���ֽ�
//
//	// ��������
//	usart_SendCmd(cmd, 20);
//}
//
///**
// * @brief    ��������
// * @param    addr   �������ַ
// * @param    o_mode ������ģʽ��0Ϊ��Ȧ�ͽ����㣬1Ϊ��Ȧ������㣬2Ϊ��Ȧ����λ��ײ���㣬3Ϊ��Ȧ����λ���ػ���
// * @param    snF   �����ͬ����־��falseΪ�����ã�trueΪ����
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF)
//{
//	uint8_t cmd[16] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x9A;                       // ������
//	cmd[2] = o_mode;                     // ����ģʽ��0Ϊ��Ȧ�ͽ����㣬1Ϊ��Ȧ������㣬2Ϊ��Ȧ����λ��ײ���㣬3Ϊ��Ȧ����λ���ػ���
//	cmd[3] = snF;                        // ���ͬ���˶���־��falseΪ�����ã�trueΪ����
//	cmd[4] = 0x6B;                       // У���ֽ�
//
//	// ��������
//	usart_SendCmd(cmd, 5);
//}
//
///**
// * @brief    ǿ���жϲ��˳�����
// * @param    addr  �������ַ
// * @retval   ��ַ + ������ + ����״̬ + У���ֽ�
// */
//void ZDT_Origin_Interrupt(uint8_t addr)
//{
//	uint8_t cmd[16] = { 0 };
//
//	// װ������
//	cmd[0] = zdt->id;                      // ��ַ
//	cmd[1] = 0x9C;                       // ������
//	cmd[2] = 0x48;                       // ������
//	cmd[3] = 0x6B;                       // У���ֽ�
//
//	// ��������
//	usart_SendCmd(cmd, 4);
//}
